#include "gait.h"
#include <math.h>

#include "sin.h"
#include "bessel.h"

#include <iostream>
using namespace std;

void GetGamma(float L, float theta, float &gamma)
{
  float L1 = 0.1296;
  float L2 = 0.232;
  float cos_param = (pow(L1, 2.0f) + pow(L, 2.0f) - pow(L2, 2.0f)) / (2.0f * L1 * L);
  if (cos_param < -1.0f)
  {
    gamma = PI;
  }
  else if (cos_param > 1.0f)
  {
    gamma = 0;
  }
  else
  {
    gamma = acos(cos_param);
  }
}

void CartesianToLegParams(float x, float y, float leg_direction, float &L, float &theta)
{
  L = pow((pow(x, 2.0f) + pow(y, 2.0f)), 0.5f);
  theta = atan2(x, y);
}

void CartesianToThetaGamma(float x, float y, float leg_direction, float &theta, float &gamma)
{
  
  float L = 0.0;
  CartesianToLegParams(x, y, leg_direction, L, theta);
  GetGamma(L, theta, gamma);
  float temp1, temp2;
  temp1 = theta + gamma - 79.3 / 180.0 * PI;
  temp2 = gamma - theta - 79.3 / 180.0 * PI;
  theta = temp1;
  gamma = temp2;
  if (leg_direction == -1.0)
  {
    theta = -theta;
    gamma = -gamma;
  }
}

void Newdog_CartesianToThetaGamma(float x, float y, float leg_direction, float &theta, float &gamma)
{
   x*=100;
   y*=100;
    float LOC=sqrt( ((pow(x,2)+pow(y,2))*15+883.6-1500-2250 )/25 );
    float LOD = sqrt(pow(x,2)+pow(y,2));
    float alpha = acos((pow(LOC,2)+88.36-225)/(18.8*LOC));
    float  costheta =(88.36+pow(LOD,2)-625)/(18.8*LOD);
    float  A = (9.4*LOD*costheta)/ y;
    float B = x/ y;
    float k =(A*B - sqrt(((pow(B,2)+1)*88.36)-pow(A,2)))/(pow(B,2)+1);
    float l = sqrt(88.36-pow(k,2));
    float  m=0.6* x+0.4*k;
    float n=0.6* y + 0.4*l;
    float beta = atan(x/y);
      theta=beta+alpha-1.57;
    gamma = -(1.57+beta-alpha);
}

void Newdog_CartesianToMotorAngle(float x, float y,float z, float leg_direction, float &theta, float &gamma,float &alpha)
{
  alpha=asin(z/y);
  y=y*cos(alpha);
  if (leg_direction==-1.0)
  {
    alpha=-alpha;
  }
  
  Newdog_CartesianToThetaGamma(x, y, leg_direction, theta, gamma);
}

void MoveLeg(LegsName leg, float t, GaitParams params, float gait_offset, float leg_direction, float &theta, float &gamma)
{
  float x; // float x for leg 0 to be set by the sin trajectory
  float y;
  //轨迹规划
  //BesselTrajectory(t, params, gait_offset, x, y);
  SinTrajectory(t, params, gait_offset, x, y);
  //运动学逆解
  //CartesianToThetaGamma(x, y, leg_direction, theta, gamma);
  Newdog_CartesianToThetaGamma(x, y, leg_direction, theta, gamma);
  
}

void Newdog_MoveLeg(LegsName leg, float t, GaitParams params, float gait_offset, float leg_direction, float &theta, float &gamma,float &alpha)
{
  float x; 
  float y;
  float z=0;
  //轨迹规划
  SinTrajectory(t, params, gait_offset, x, y);
  //运动学逆解
  Newdog_CartesianToMotorAngle(x, y, z,leg_direction, theta, gamma,alpha);
  
}

void gait(GaitParams params, float leg0_offset, float leg1_offset, float leg2_offset, float leg3_offset)
{
  GaitParams paramsR = params;
  GaitParams paramsL = params;
  paramsR.step_length -= params.step_diff;
  paramsL.step_length += params.step_diff;

  static float t = 0.0f;
  t += 0.015f;
  const float leg0_direction = 1.0;
  //MoveLeg(LF, t, paramsL, leg0_offset, leg0_direction, cmd_data[LF][FrontPart], cmd_data[LF][HindPart]);
  Newdog_MoveLeg(LF, t, paramsL, leg0_offset, leg0_direction, cmd_data[LF][FrontPart], cmd_data[LF][HindPart],cmd_data[LF][Abduction]);

  const float leg1_direction = -1.0;
  //MoveLeg(LH, t, paramsL, leg1_offset, leg1_direction, cmd_data[LH][FrontPart], cmd_data[LH][HindPart]);
  Newdog_MoveLeg(LH, t, paramsL, leg1_offset, leg1_direction, cmd_data[LH][FrontPart], cmd_data[LH][HindPart],cmd_data[LH][Abduction]);

  const float leg2_direction =1.0;
  //MoveLeg(RF, t, paramsR, leg2_offset, leg2_direction, cmd_data[RF][FrontPart], cmd_data[RF][HindPart]);
  Newdog_MoveLeg(RF, t, paramsR, leg2_offset, leg2_direction, cmd_data[RF][FrontPart], cmd_data[RF][HindPart],cmd_data[RF][Abduction]);

  const float leg3_direction = -1.0;
  //MoveLeg(RH, t, paramsR, leg3_offset, leg3_direction, cmd_data[RH][FrontPart], cmd_data[RH][HindPart]);
  Newdog_MoveLeg(RH, t, paramsR, leg3_offset, leg3_direction, cmd_data[RH][FrontPart], cmd_data[RH][HindPart],cmd_data[RH][Abduction]);

}
